import time
import math
import smbus
import RPi.GPIO as GPIO


class PCA9685:
    # Registers/etc.
    __SUBADR1 = 0x02
    __SUBADR2 = 0x03
    __SUBADR3 = 0x04
    __MODE1 = 0x00
    __PRESCALE = 0xFE
    __LED0_ON_L = 0x06
    __LED0_ON_H = 0x07
    __LED0_OFF_L = 0x08
    __LED0_OFF_H = 0x09
    __ALLLED_ON_L = 0xFA
    __ALLLED_ON_H = 0xFB
    __ALLLED_OFF_L = 0xFC
    __ALLLED_OFF_H = 0xFD

    def __init__(self, address, debug=False):
        self.bus = smbus.SMBus(1)
        self.address = address
        self.debug = debug
        if (self.debug):
            print("Reseting PCA9685")
        self.write(self.__MODE1, 0x00)

    def write(self, reg, value):
        "Writes an 8 - bit value to the specified register/address"
        self.bus.write_byte_data(self.address, reg, value)
        if (self.debug):
            print("I2C: Write 0x%02X to register 0x%02X" % (value, reg))

    def read(self, reg):
        "Read an unsigned byte from the I2C device"
        result = self.bus.read_byte_data(self.address, reg)
        if (self.debug):
            print("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % (self.address, result & 0xFF, reg))
        return result

    def setPWMFreq(self, freq):
        "Sets the PWM frequency"
        prescaleval = 25000000.0  # 25MHz
        prescaleval /= 4096.0  # 12 - bit
        prescaleval /= float(freq)
        prescaleval -= 1.0
        if (self.debug):
            print("Setting PWM frequency to %d Hz" % freq)
            print("Estimated pre - scale: %d" % prescaleval)
        prescale = math.floor(prescaleval + 0.5)
        if (self.debug):
            print("Final pre - scale: %d" % prescale)

        oldmode = self.read(self.__MODE1)
        newmode = (oldmode & 0x7F) | 0x10  # sleep
        self.write(self.__MODE1, newmode)  # go to sleep
        self.write(self.__PRESCALE, int(math.floor(prescale)))
        self.write(self.__MODE1, oldmode)
        time.sleep(0.005)
        self.write(self.__MODE1, oldmode | 0x80)

    def setPWM(self, channel, on, off):
        "Sets a single PWM channel"
        self.write(self.__LED0_ON_L + 4 * channel, on & 0xFF)
        self.write(self.__LED0_ON_H + 4 * channel, on >> 8)
        self.write(self.__LED0_OFF_L + 4 * channel, off & 0xFF)
        self.write(self.__LED0_OFF_H + 4 * channel, off >> 8)
        if (self.debug):
            print("channel: %d  LED_ON: %d LED_OFF: %d" % (channel, on, off))

    def setDutycycle(self, channel, pulse):
        self.setPWM(channel, 0, int(pulse * (4096 / 100)))

    def setLevel(self, channel, value):
        if (value == 1):
            self.setPWM(channel, 0, 4095)
        else:
            self.setPWM(channel, 0, 0)


# 假设 PWM9 对应的 PCA9685 通道
HORIZONTAL_SERVO_CHANNEL = 12
# 假设 PWM10 对应的 PCA9685 通道
VERTICAL_SERVO_CHANNEL = 10

# 初始化 PCA9685
pwm = PCA9685(0x40, debug=False)
pwm.setPWMFreq(50)


# 调整舵机角度
def set_servo_angle(channel, angle):
    # 舵机角度范围 0 - 180 度，对应占空比 2.5 - 12.5
    # PCA9685 的占空比范围是 0 - 4095
    pulse = int((angle / 18 + 2.5) / 100 * 4095)
    pwm.setPWM(channel, 0, pulse)
    time.sleep(0.3)  # 等待舵机转动到指定角度


# 调整水平舵机角度
def adjustHorizontalAngle(angle):
    set_servo_angle(HORIZONTAL_SERVO_CHANNEL, angle)


# 调整竖直舵机角度
def adjustVerticalAngle(angle):
    set_servo_angle(VERTICAL_SERVO_CHANNEL, angle)


try:
    # 示例：将水平舵机调整到 90 度
    adjustHorizontalAngle(90)

    # 示例：将竖直舵机调整到 45 度
    adjustVerticalAngle(45)

    # 示例：将水平舵机再调整到 120 度
    adjustHorizontalAngle(120)

    # 示例：将竖直舵机调整到 135 度
    adjustVerticalAngle(135)

except KeyboardInterrupt:
    print("程序被用户中断")
finally:
    # 不需要额外的清理操作，PCA9685会保持状态
    pass